function [cost_map] = SensingCostMap(weather,aircraft,time,alt,Jv)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

t=floor((time-weather.t0)./weather.ts) +1;

if (t < 1) time=0; end
if (t > weather.tnum) time = weather.tf; end;

frame = weather.frame;
shift_x = floor((frame.u * (time - weather.t0))/weather.xs);
shift_y = floor((frame.v * (time - weather.t0))/weather.ys);
%  cost_map( = cost_map(:,:);
%  return;

%fname = sprintf('bin/[cost_map]_%s_%ds_%dm_%dms.mat',weather.source,time-mod(time,60),aircraft.z,aircraft.velocity);
fname = sprintf('bin/[sensing_cost_map]_%s_%ds_%dms.mat',weather.source,time-mod(time,60),aircraft.velocity);

if exist(fname) == 2
  load(fname);
  return;
end;

%2d approximation
k=floor((alt-weather.z0)/weather.zs) +1;

pos = aircraft.position;
target_x = floor((pos(1)-weather.x0)/weather.xs) +1;
target_y = floor((pos(2)-weather.y0)/weather.ys) +1;

%fig=3;
%figure(fig); clf;
%title('Wavefront');
%grid;

%--[ step 1 ]--%
cost_map = Inf*ones(weather.xnum,weather.ynum);
far = ones(weather.xnum,weather.ynum);
considered = 0*far;
accepted = 0*far;

%--[ step 2 ]--%
point = [target_x, target_y];
accepted(point(1)-shift_x,point(2)-shift_y) = 1;
far(point(1)-shift_x,point(2)-shift_y) = 0;
cost_map(point(1)-shift_x,point(2)-shift_y) = 0;

%--[ step 3 ]--%
for i = point(1)-1:point(1)+1
  for j = point(2)-1:point(2)+1
    if i <= (weather.xnum+shift_x) && i > (0+shift_x) && j <= (weather.ynum+shift_y) && j > (0+shift_y)
      if accepted(i-shift_x,j-shift_y) == 0
        considered(i-shift_x,j-shift_y) = 1;
        cost_map(i-shift_x,j-shift_y) = CalcSensingCost(i,j,point(1),point(2),weather,aircraft,time,alt,cost_map,Jv);
        far(i-shift_x,j-shift_y) = 0;
      end
    end
  end
end

%figure(fig);clf;
%PlotGrid(accepted,'og',[0 1 0],fig);
%PlotGrid(considered,'^b',[0 0 1],fig);
%PlotGrid(far,'dr',[1 0 0],fig);
%axis([0,weather.xnum,0,weather.ynum]);
%pause(0.1);

h = waitbar(0,'Caluclating Cost');

while max(max(considered)) == 1
  %--[ step 4 ]--%
  min_x = 0;
  min_y = 0;
  min_val = Inf;
  for i = 1:weather.xnum
    for j = 1:weather.ynum
      if considered(i,j) == 1
         if cost_map(i,j) <= min_val
           min_val = cost_map(i,j);
           min_x = i;
           min_y = j;
         end
      end
    end
  end
  point = [min_x+shift_x,min_y+shift_y];

  %--[ step 5 ]--%
  accepted(point(1)-shift_x,point(2)-shift_y) = 1;
  considered(point(1)-shift_x,point(2)-shift_y) = 0;

  %--[ step 6 ]--%
  %--[ step 7 ]--%
  for i = point(1)-1:point(1)+1
    for j = point(2)-1:point(2)+1
      if i <= (weather.xnum+shift_x) && i > (0+shift_x) && j <= (weather.ynum+shift_y) && j > (0+shift_y)
        if accepted(i-shift_x,j-shift_y) == 0
          if considered(i-shift_x,j-shift_y) == 1
              cost_map(i-shift_x,j-shift_y) = min(cost_map(i-shift_x,j-shift_y),CalcSensingCost(i,j,point(1),point(2),weather,aircraft,time,alt,cost_map,Jv));
          else
            considered(i-shift_x,j-shift_y) = 1;
            cost_map(i-shift_x,j-shift_y) = CalcSensingCost(i,j,point(1),point(2),weather,aircraft,time,alt,cost_map,Jv);
            far(i-shift_x,j-shift_y) = 0;
          end
        end
      end
    end
  end

%  figure(fig); hold off;
%  PlotGrid(accepted,'og',[0 1 0],fig);
%  axis([0,weather.xnum,0,weather.ynum]);
%  hold on;
%  PlotGrid(considered,'^b',[0 0 1],fig);
%  PlotGrid(far,'dr',[1 0 0],fig);
%  pause(0.1)

  %--[ step 8 ]--%
  waitbar(sum(sum(accepted))/(weather.xnum*weather.ynum),h);
end

close(h);
clear h;

%save(fname,'cost_map');
